/* 
 * pwm4Motor.c - The C source file of DC-Motor(PWM) devices
 * NOTE: This file is based on HAL library of stm32 platform
 *          电机PWM设备BSP接口实现定义
 *          接口声明功能注释见BSP.h
 *          直流电机驱动是针对H桥驱动L298写的！
 *
 * Copyright (c) 2020-, FOSH Project
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author            Notes           mail
 * 2021-04-02     江榕煜       first version   2373180028@qq.com
**/

#include "BSP.h"

#ifdef __USER_DEBUG__   // 全局调试打印使能

#define __USER_DEBUG__IMU__

#endif
//#define __USER_DEBUG__MOTOR__ //手动启动单项调试打印

//static TIM_HandleTypeDef * MOTOR_TIMER_HANDLER[] = {
//    &htim1,&htim15
//};
static uint32_t MOTOR_TIMER_COUNTER[] = {
    Motor_1_PWMcounter,Motor_2_PWMcounter
};

/** @brief  直流电机初始化
 *  @note   调用前先初始化PWM设备
 *  @author 江榕煜（2021.4.2）
 *  @param  
 *          None
 *  @retval 
 *          None
**/
void DCMotor_Init(void)
{
    HAL_TIM_PWM_Start(&htim1,   TIM_CHANNEL_1); // 启动PWM输出
    HAL_TIM_PWM_Start(&htim1,   TIM_CHANNEL_2); // 启动PWM输出
    
    HAL_TIM_PWM_Start(&htim15,  TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim15,  TIM_CHANNEL_2);
}

/** @brief  直流电机速度设置
 *  @note   使用前必使用 DCMotor_Init() 方法
 *          速度百分比范围：-100%~+100%
 *  @author 江榕煜（2021.4.2）
 *  @param
 *          channel(uint32_t)   电机通道号
 *          duty(float)         占空比
 *  @retval 
 *          None
**/
void DCMotor_SetSpeed(uint32_t channel,float duty)
{
//    if(duty > 100)
//        duty = 100;
//    else if(duty < -100)
//        duty = -100;
    duty /= 100;    // 百分数转小数
    int pulse = duty*MOTOR_TIMER_COUNTER[channel];
    #ifdef __USER_DEBUG__MOTOR__
    rt_kprintf("set pulse is %d \n",pulse);
    #endif
    switch(channel)
    {
        case(1):    //通道1
        {
            if(duty >= 0)
            {
//                __HAL_TIM_SET_COMPARE(&htim15,TIM_CHANNEL_1,0);  // 使用宏函数太慢了，直接寄存器操作
                
                htim15.Instance->CCR1 = 0;  //正转
                htim15.Instance->CCR2 = pulse;
            }
            else{
                htim15.Instance->CCR1 = -pulse;      //反转
                htim15.Instance->CCR2 = 0;
            }
            break;
        }
        
        case(0):    // 通道0
        {
            if(duty >=0)
            {
                htim1.Instance->CCR1 = 0;
                htim1.Instance->CCR2 = pulse;
            }
            else{
                htim1.Instance->CCR1 = -pulse;
                htim1.Instance->CCR2 = 0;
            }
            break;
        }
        
        default:
        {
            #ifdef __USER_DEBUG__MOTOR__
            rt_kprintf("not useful channel\n");
            #endif
        }
    }
}

/** @brief  关闭直流电机PWM设备
 *  @note   要恢复需调用 DCMotor_Init() 方法
 *  @author 江榕煜（2021.4.2）
 *  @param
 *          None
 *  @retval 
 *          None
**/
void DCMotor_Unload(void)
{
    HAL_TIM_PWM_Stop(&htim1,    TIM_CHANNEL_1);
    HAL_TIM_PWM_Stop(&htim1,    TIM_CHANNEL_2);
    HAL_TIM_PWM_Stop(&htim15,   TIM_CHANNEL_1);
    HAL_TIM_PWM_Stop(&htim15,   TIM_CHANNEL_2);
}

#ifdef __USER_DEBUG__MOTOR__

/** @brief  启动电机PWM
 *  @note   测试用
 *  @author 江榕煜（2021.4.2）
 *  @param  None
 *  @retval None
**/
int beginM()
{
    DCMotor_Init();
    return 0;
}
MSH_CMD_EXPORT(beginM, enable motor output);

/** @brief  设置占空比
 *  @note   测试用
 *  @author 江榕煜（2021.4.2）
 *  @param  None
 *  @retval None
**/
int setM(int argn,char ** argv)
{
    if(argn<3)
    {
        rt_kprintf("format: setM -channel -duty");
        return -1;
    }
    uint32_t channel;
    float duty;
    sscanf(argv[1],"%d",&channel);
    sscanf(argv[2],"%f",&duty);
    DCMotor_SetSpeed(channel,duty);
    
    return 0;
}
MSH_CMD_EXPORT(setM, set motor duty);

/** @brief  关闭电机PWM
 *  @note   测试用
 *  @author 江榕煜（2021.4.2）
 *  @param  None
 *  @retval None
**/
int endM()
{
    DCMotor_Unload();
    return 0;
}
MSH_CMD_EXPORT(endM, disable motor output);


#endif
